﻿using UnityEngine;

namespace HXu.MyTools
{
    /// <summary>
    /// QuaternionTools 针对四元数Quaternion的工具 <br />
    /// /************************************************* <br />
    /// -- 内容：<br />
    ///     1A、四元数平滑阻尼函数（仿Unity的Vector3.SmoothDamp实现）<br />
    /// <br />
    /// -- 使用方法：<br />
    ///     1、引用命名空间，直接调用。<br />
    /// *************************************************/ <br />
    /// </summary>
    public static class QuaternionTools
    {
        /// <summary>
        /// 1A、四元数平滑阻尼函数（仿Unity的Vector3.SmoothDamp实现）
        /// </summary>
        /// <param name="current">当前旋转值</param>
        /// <param name="target">目标旋转值</param>
        /// <param name="currentVelocity">当前角速度（会被方法修改）</param>
        /// <param name="smoothTime">达到目标的大致时间（秒），较小值更快达到目标</param>
        /// <param name="maxSpeed">最大旋转速度（角度/秒），默认无限大</param>
        /// <param name="deltaTime">时间间隔（秒），默认0.02s（50FPS）</param>
        /// <returns>经过平滑处理后的新旋转值</returns>
        public static Quaternion SmoothDamp(this Quaternion current, Quaternion target, ref Quaternion currentVelocity, float smoothTime, float maxSpeed = Mathf.Infinity, float deltaTime = 0.02f)
        {
            // 计算当前旋转与目标旋转之间的角度差（单位：度）
            float deltaAngle = Quaternion.Angle(current, target);

            // 当角度差非常小时（小于0.001度），直接跳转到目标旋转
            if (deltaAngle <= 0.001f)
            {
                currentVelocity = Quaternion.identity; // 重置角速度为无旋转
                return target;
            }

            // 使用Unity的平滑阻尼函数计算插值比例因子（使用四元数x分量作为速度参数）
            // 注意：这里将四元数的x分量作为速度参数可能不够严谨，实际工程中建议使用单独的速度变量
            float t = Mathf.SmoothDamp(0, 1, ref currentVelocity.x, smoothTime, maxSpeed, deltaTime);

            // 使用球面线性插值返回平滑后的旋转
            // Slerp可以保持旋转速度的恒定，适合处理旋转插值
            return Quaternion.Slerp(current, target, t);
        }
    }
}